import { RVOMath, Obstacle, Vector2 } from "./Common";
import { Simulator } from "./Simulator";
import { Agent } from "./Agent";
import { toFixed } from "../common/Util";

class FloatPair {
    a: number;
    b: number;
    constructor(a: number, b: number) {
        this.a = a;
        this.b = b;
    }

    lessThan(rhs: FloatPair) {//小于
        return this.a < rhs.a || !(rhs.a < this.a) && this.b < rhs.b;
    }

    lessEqualThan(rhs: FloatPair) {//小于等于
        return (this.a == rhs.a && this.b == rhs.b) || this.lessThan(rhs);
    }

    bigThan(rhs: FloatPair) {//大于
        return !this.lessEqualThan(rhs);
    }

    bigEqualThan(rhs: FloatPair) {//大于等于
        return !this.lessThan(rhs);
    }
}

class AgentTreeNode {
    begin: number = 0;
    end: number = 0;
    left: number = 0;
    right: number = 0;
    maxX: number = 0;
    maxY: number = 0;
    minX: number = 0;
    minY: number = 0;
}

class ObstacleTreeNode {
    constructor(
        public obstacle: Obstacle,
        public left: ObstacleTreeNode | null,
        public right: ObstacleTreeNode | null,
    ) {

    }
}

export class KdTree {
    /**
     * The maximum size of an agent k-D tree leaf.
     */
    MAX_LEAF_SIZE = 10;
    agents: Agent[] = [];
    agentTree: AgentTreeNode[] = [];
    obstacleTree: ObstacleTreeNode | any = null
    simulator: Simulator
    constructor(simulator: Simulator) {
        this.simulator = simulator
    }
    buildAgentTree(agentNum: number) {
        if (!this.agents || this.agents.length != agentNum) {
            this.agents = new Array<Agent>(agentNum);
            for (let i = 0; i < this.agents.length; i++) {
                this.agents[i] = this.simulator.getAgent(i);
            }

            this.agentTree = new Array<AgentTreeNode>(2 * this.agents.length);
            for (let i = 0; i < this.agentTree.length; i++) {
                this.agentTree[i] = new AgentTreeNode();
            }
        }

        if (this.agents.length != 0) {
            this.buildAgentTreeRecursive(0, this.agents.length, 0);
        }
    }

    buildObstacleTree() {
        let obstacles = new Array<Obstacle>(this.simulator.obstacles.length);
        for (let i = 0; i < obstacles.length; i++) {
            obstacles[i] = this.simulator.obstacles[i];
        }
        this.obstacleTree = this.buildObstacleTreeRecursive(obstacles);
    }

    computeAgentNeighbors(agent: Agent, rangeSq: number) {
        return this.queryAgentTreeRecursive(agent, rangeSq, 0);
    }

    computeObstacleNeighbors(agent: Agent, rangeSq: number) {
        this.queryObstacleTreeRecursive(agent, rangeSq, this.obstacleTree);
    }

    queryVisibility(q1: Vector2, q2: Vector2, radius: number) {
        return this.queryVisibilityRecursive(q1, q2, radius, this.obstacleTree);
    }

    buildAgentTreeRecursive(begin: number, end: number, node: number) {
        this.agentTree[node].begin = begin;
        this.agentTree[node].end = end;
        this.agentTree[node].minX = this.agentTree[node].maxX = this.agents[begin].position_.x;
        this.agentTree[node].minY = this.agentTree[node].maxY = this.agents[begin].position_.y;

        for (let i = begin + 1; i < end; ++i) {
            this.agentTree[node].maxX = Math.max(this.agentTree[node].maxX, this.agents[i].position_.x);
            this.agentTree[node].minX = Math.min(this.agentTree[node].minX, this.agents[i].position_.x);
            this.agentTree[node].maxY = Math.max(this.agentTree[node].maxY, this.agents[i].position_.y);
            this.agentTree[node].minY = Math.min(this.agentTree[node].minY, this.agents[i].position_.y);
        }

        if (end - begin > this.MAX_LEAF_SIZE) {
            // no leaf node
            let isVertical = (this.agentTree[node].maxX - this.agentTree[node].minX) > (this.agentTree[node].maxY - this.agentTree[node].minY);
            let splitValue = 0.5 * (isVertical ? this.agentTree[node].maxX + this.agentTree[node].minX : this.agentTree[node].maxY + this.agentTree[node].minY);

            let left = begin;
            let right = end;

            while (left < right) {
                while (left < right && (isVertical ? this.agents[left].position_.x : this.agents[left].position_.y) < splitValue) {
                    ++left;
                }

                while (right > left && (isVertical ? this.agents[right - 1].position_.x : this.agents[right - 1].position_.y) >= splitValue) {
                    --right;
                }

                if (left < right) {
                    let tmp = this.agents[left];
                    this.agents[left] = this.agents[right - 1];
                    this.agents[right - 1] = tmp;
                    ++left;
                    --right;
                }
            }

            let leftSize = left - begin;
            if (leftSize == 0) {
                ++leftSize;
                ++left;
                ++right;
            }

            this.agentTree[node].left = node + 1;
            this.agentTree[node].right = node + 2 * leftSize;

            this.buildAgentTreeRecursive(begin, left, this.agentTree[node].left);
            this.buildAgentTreeRecursive(left, end, this.agentTree[node].right);
        }
    }


    buildObstacleTreeRecursive(obstacles: Obstacle[]): ObstacleTreeNode | null {
        if (obstacles.length == 0) {
            return null;
        } else {
            let optimalSplit = 0;
            let minLeft = obstacles.length;
            let minRight = minLeft;

            for (let i = 0; i < obstacles.length; ++i) {
                let leftSize = 0;
                let rightSize = 0;

                let obstacleI1 = obstacles[i];
                let obstacleI2 = obstacleI1.next;
                if (obstacleI2) {
                    for (let j = 0; j < obstacles.length; j++) {
                        if (i == j) {
                            continue;
                        }

                        let obstacleJ1 = obstacles[j];
                        let obstacleJ2 = obstacleJ1.next;
                        if (obstacleJ2) {
                            let j1LeftOfI = RVOMath.leftOf(obstacleI1.point, obstacleI2.point, obstacleJ1.point);
                            let j2LeftOfI = RVOMath.leftOf(obstacleI1.point, obstacleI2.point, obstacleJ2.point);

                            if (j1LeftOfI >= -RVOMath.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON) {
                                ++leftSize;
                            }
                            else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON) {
                                ++rightSize;
                            }
                            else {
                                ++leftSize;
                                ++rightSize;
                            }

                            let fp1 = new FloatPair(Math.max(leftSize, rightSize), Math.min(leftSize, rightSize));
                            let fp2 = new FloatPair(Math.max(minLeft, minRight), Math.min(minLeft, minRight));

                            if (fp1.bigEqualThan(fp2)) {
                                break;
                            }
                        }
                    }

                    let fp1 = new FloatPair(Math.max(leftSize, rightSize), Math.min(leftSize, rightSize));
                    let fp2 = new FloatPair(Math.max(minLeft, minRight), Math.min(minLeft, minRight));

                    if (fp1.lessThan(fp2)) {
                        minLeft = leftSize;
                        minRight = rightSize;
                        optimalSplit = i;
                    }
                }
            }
            {
                /* Build split node. */
                let leftObstacles: Obstacle[] = [];
                for (let n = 0; n < minLeft; ++n) leftObstacles.push();

                let rightObstacles: Obstacle[] = [];
                for (let n = 0; n < minRight; ++n) rightObstacles.push();

                let leftCounter = 0;
                let rightCounter = 0;
                let i = optimalSplit;

                let obstacleI1 = obstacles[i];
                let obstacleI2 = obstacleI1.next;
                if (obstacleI2) {

                    for (let j = 0; j < obstacles.length; ++j) {
                        if (i == j) {
                            continue;
                        }

                        let obstacleJ1 = obstacles[j];
                        let obstacleJ2 = obstacleJ1.next;
                        if (obstacleJ2) {
                            let j1LeftOfI = RVOMath.leftOf(obstacleI1.point, obstacleI2.point, obstacleJ1.point);
                            let j2LeftOfI = RVOMath.leftOf(obstacleI1.point, obstacleI2.point, obstacleJ2.point);

                            if (j1LeftOfI >= -RVOMath.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON) {
                                leftObstacles[leftCounter++] = obstacles[j];
                            }
                            else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON) {
                                rightObstacles[rightCounter++] = obstacles[j];
                            }
                            else {
                                /* Split obstacle j. */
                                let t = RVOMath.det(obstacleI2.point.minus(obstacleI1.point), obstacleJ1.point.minus(obstacleI1.point)) /
                                    RVOMath.det(obstacleI2.point.minus(obstacleI1.point), obstacleJ1.point.minus(obstacleJ2.point));

                                let splitpoint = obstacleJ1.point.plus((obstacleJ2.point.minus(obstacleJ1.point)).scale(t));

                                let newObstacle = new Obstacle();
                                newObstacle.point = splitpoint;
                                newObstacle.previous = obstacleJ1;
                                newObstacle.next = obstacleJ2;
                                newObstacle.convex = true;
                                newObstacle.direction = obstacleJ1.direction;

                                newObstacle.id = this.simulator.obstacles.length;

                                this.simulator.obstacles.push(newObstacle);

                                obstacleJ1.next = newObstacle;
                                obstacleJ2.previous = newObstacle;

                                if (j1LeftOfI > 0.0) {
                                    leftObstacles[leftCounter++] = obstacleJ1;
                                    rightObstacles[rightCounter++] = newObstacle;
                                }
                                else {
                                    rightObstacles[rightCounter++] = obstacleJ1;
                                    leftObstacles[leftCounter++] = newObstacle;
                                }
                            }
                        }
                    }
                }
                let node = new ObstacleTreeNode(
                    obstacleI1,
                    this.buildObstacleTreeRecursive(leftObstacles),
                    this.buildObstacleTreeRecursive(rightObstacles)
                );
                return node;
            }
        }
    }


    queryAgentTreeRecursive(agent: Agent, rangeSq: number, node: number) {
        if (this.agentTree[node].end - this.agentTree[node].begin <= this.MAX_LEAF_SIZE) {
            for (let i = this.agentTree[node].begin; i < this.agentTree[node].end; ++i) {
                rangeSq = agent.insertAgentNeighbor(this.agents[i], rangeSq);
            }
        }
        else {
            let distSqLeft = RVOMath.sqr(Math.max(0, this.agentTree[this.agentTree[node].left].minX - agent.position_.x)) +
                RVOMath.sqr(Math.max(0, agent.position_.x - this.agentTree[this.agentTree[node].left].maxX)) +
                RVOMath.sqr(Math.max(0, this.agentTree[this.agentTree[node].left].minY - agent.position_.y)) +
                RVOMath.sqr(Math.max(0, agent.position_.y - this.agentTree[this.agentTree[node].left].maxY));

            let distSqRight = RVOMath.sqr(Math.max(0, this.agentTree[this.agentTree[node].right].minX - agent.position_.x)) +
                RVOMath.sqr(Math.max(0, agent.position_.x - this.agentTree[this.agentTree[node].right].maxX)) +
                RVOMath.sqr(Math.max(0, this.agentTree[this.agentTree[node].right].minY - agent.position_.y)) +
                RVOMath.sqr(Math.max(0, agent.position_.y - this.agentTree[this.agentTree[node].right].maxY));

            if (distSqLeft < distSqRight) {
                if (distSqLeft < rangeSq) {
                    rangeSq = this.queryAgentTreeRecursive(agent, rangeSq, this.agentTree[node].left);

                    if (distSqRight < rangeSq) {
                        rangeSq = this.queryAgentTreeRecursive(agent, rangeSq, this.agentTree[node].right);
                    }
                }
            }
            else {
                if (distSqRight < rangeSq) {
                    rangeSq = this.queryAgentTreeRecursive(agent, rangeSq, this.agentTree[node].right);

                    if (distSqLeft < rangeSq) {
                        rangeSq = this.queryAgentTreeRecursive(agent, rangeSq, this.agentTree[node].left);
                    }
                }
            }

        }
        return rangeSq;
    }

    // pass ref range
    queryObstacleTreeRecursive(agent: Agent, rangeSq: number, node: ObstacleTreeNode | null): number {
        if (node) {
            let obstacle1 = node.obstacle;
            let obstacle2 = obstacle1.next;
            if (obstacle2) {
                let agentLeftOfLine = RVOMath.leftOf(obstacle1.point, obstacle2.point, agent.position_);

                rangeSq = this.queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0 ? node.left : node.right));

                let distSqLine = toFixed(RVOMath.sqr(agentLeftOfLine) / RVOMath.absSq(obstacle2.point.minus(obstacle1.point)));

                if (distSqLine < rangeSq) {
                    if (agentLeftOfLine < 0) {
                        /*
                         * Try obstacle at this node only if is on right side of
                         * obstacle (and can see obstacle).
                         */
                        agent.insertObstacleNeighbor(node.obstacle, rangeSq);
                    }

                    /* Try other side of line. */
                    this.queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0 ? node.right : node.left));
                }
            }
            return rangeSq;
        }
        return rangeSq;

    }



    queryVisibilityRecursive(q1: Vector2, q2: Vector2, radius: number, node: ObstacleTreeNode | null): any {
        if (node) {
            let obstacle1 = node.obstacle;
            let obstacle2 = obstacle1.next;
            if (obstacle2 && node.left) {
                let q1LeftOfI = RVOMath.leftOf(obstacle1.point, obstacle2.point, q1);
                let q2LeftOfI = RVOMath.leftOf(obstacle1.point, obstacle2.point, q2);
                let invLengthI = toFixed(1.0 / RVOMath.absSq(obstacle2.point.minus(obstacle1.point)));

                if (q1LeftOfI >= 0 && q2LeftOfI >= 0) {
                    return this.queryVisibilityRecursive(q1, q2, radius, node.left) && ((toFixed(RVOMath.sqr(q1LeftOfI) * invLengthI) >= RVOMath.sqr(radius) && toFixed(RVOMath.sqr(q2LeftOfI) * invLengthI) >= RVOMath.sqr(radius)) || this.queryVisibilityRecursive(q1, q2, radius, node.right));
                }
                else if (q1LeftOfI <= 0 && q2LeftOfI <= 0) {
                    return this.queryVisibilityRecursive(q1, q2, radius, node.right) && ((toFixed(RVOMath.sqr(q1LeftOfI)) * invLengthI >= RVOMath.sqr(radius) && toFixed(RVOMath.sqr(q2LeftOfI) * invLengthI) >= RVOMath.sqr(radius)) || this.queryVisibilityRecursive(q1, q2, radius, node.left));
                }
                else if (q1LeftOfI >= 0 && q2LeftOfI <= 0) {
                    /* One can see through obstacle from left to right. */
                    return this.queryVisibilityRecursive(q1, q2, radius, node.left) && this.queryVisibilityRecursive(q1, q2, radius, node.right);
                }
                else {
                    let point1LeftOfQ = RVOMath.leftOf(q1, q2, obstacle1.point);
                    let point2LeftOfQ = RVOMath.leftOf(q1, q2, obstacle2.point);
                    let invLengthQ = toFixed(1.0 / RVOMath.absSq(q2.minus(q1)));

                    return (toFixed(point1LeftOfQ * point2LeftOfQ) >= 0 && toFixed(RVOMath.sqr(point1LeftOfQ) * invLengthQ) > RVOMath.sqr(radius) && toFixed(RVOMath.sqr(point2LeftOfQ) * invLengthQ) > RVOMath.sqr(radius) && this.queryVisibilityRecursive(q1, q2, radius, node.left) && this.queryVisibilityRecursive(q1, q2, radius, node.right));
                }
            }
        }
        return true;
    }
}        